%Code to randomise placement of some object and then to detect the
%placement of it through MLE
%Author: Nathan Rich
clear
clc

x=randi(100); %Randomise Placement
y=randi(100);

for I = 1:20 %Loop to send transmissions with noise
    nx = [(-1)^(randi(2))]*cos(rand()); %noise, with zero mean and some variance
    ny = [(-1)^(randi(2))]*cos(rand()); 
    X(I) = x + nx; %signals received
    Y(I) = y + ny;
end

%MLE code to detect placement
%==========================================================================
%^^^^^^^^ Need to know what type of distribution it is, Since we are using
%gaussian white Noise Assume it is gaussian^^^^^^^^^^^^
%==========================================================================
% n = x,  m = y
syms n xsigma xmean; % Sets value, standard deviation and mean as symbols

f = 1/(sqrt(2*pi*xsigma^2)) * exp(- (n-xmean)^2/(2*xsigma^2)); %gaussian distribution
Lx = 1; %likelihood x

for J = 1:length(X) %Sums all probabilities
    Lx = Lx *subs(f ,n ,X(J)); %substitutes each x value into f
end

lx = log(Lx); %log likelihood x
F = diff(lx,xmean);

xMLE = double(solve(F,xmean));

syms m ysigma ymean; % Sets value, standard deviation and mean as symbols

g = 1/(sqrt(2*pi*ysigma^2)) * exp(- (m-ymean)^2/(2*ysigma^2)); %gaussian distribution
Ly = 1; %likelihood

for J = 1:length(Y) %Sums all probabilities
    Ly = Ly *subs(g ,m ,Y(J)); %substitutes each y value into g
end

ly = log(Ly); %log likelihood y
G = diff(ly,ymean);

yMLE = double(solve(G,ymean));

xMLEerror = abs(x - xMLE);
yMLEerror = abs(y - yMLE);

fprintf('The error using MLE in the x direction is %d.\nThe error using MLE in the y direction is %d.\n',xMLEerror,yMLEerror)



%MoM Code to detect placement
xMoM = mean(X);
yMoM = mean(Y);
xMoMerror = abs(x - xMoM);
yMoMerror = abs(y - yMoM);
fprintf('The error using MoM in the x direction is %d.\nThe error using MoM in the y direction is %d.\n',xMoMerror,yMoMerror)

xMoMerrorplot(1) = xMoM; %Setting up arrays for error plot
xMoMerrorplot(2) = x;
yMoMerrorplot(1) = yMoM;
yMoMerrorplot(2) = y;

figure(1)
plot(xMoMerrorplot, yMoMerrorplot, '-o')
title('Error')
xlabel('x')
ylabel('y')
legend('MoM Error')